
/*
    Ultrasonic
*/
#ifndef UC_H
#define UC_H

#include "component_drv8833_pro_mini.h"
#include <HCSR04.h>

int trigPin = 12; // Trigger
int echoPin = 13; // Echo
float distance = 0.0;
float avoidance_distance = 30.0;

long previousMillis_ul;
const int interval_ul = 100;

void setup_ultrasonic()
{
  HCSR04.begin(trigPin, echoPin);
}

void loop_ultrasonic()
{
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis_ul >= interval_ul)
  {
    previousMillis_ul = currentMillis;              // Remember the time
    double *distances = HCSR04.measureDistanceCm(); // read distance
    distance = distances[0];
    Serial.print("UL ");
    Serial.println(distance);
    if ((distance != -1.0) && (distance < avoidance_distance))
    {
      if (car_state != CAR_BACKWARD & car_state != CAR_STOP)
      {
        cur_car_cmd = CAR_STOP;
        car_stop();
        car_state = CAR_STOP;
      }
    }
  }
}

#endif /* UC_H */